using System;
using System.Collections.Generic;
using System.Text;
using Mathematics.Matrice;

namespace Photogrammetry.Transformation
{
    /// <summary>
    /// This is the temporary place for struct point
    /// Point structure should be changed to a class with some features
    /// </summary>
    public struct point
    {
        public double X;
        public double Y;
        public double Z; // Z will be ommitted in this program but for feature
    }
    /// <summary>
    /// This class calculates the transformation parameters and transformed coordinates
    /// of the given coordinates.
    /// </summary>
    public class Projective
    {
        private double[] A = new double[3];
        private double[] B = new double[3];
        private double[] C = new double[3];
        private double[] D = new double[3];
        private double[] E = new double[3];
        private double[] F = new double[3];
        private double[] G = new double[3];
        private double[] P = new double[6];
        private Matrice c = new Matrice(8, 1);
        private Matrice oldc = new Matrice(8, 1);
        /// <summary>
        /// This method calculates the transformation parameters using the first 4 coordinates given by the parameters.
        /// </summary>
        /// <param name="pictureVertices">
        /// Coordinates of the picture by using an array of point structure.
        /// </param>
        /// <param name="realVertices">
        /// Real coordinates by using an array of point structure.
        /// </param>
        private void ProjectiveUnControlled(point[] pictureVertices, point[] realVertices)
        {
            A[0] = (pictureVertices[1].Y - pictureVertices[0].Y) / (pictureVertices[1].X - pictureVertices[0].X);
            A[1] = (pictureVertices[2].Y - pictureVertices[0].Y) / (pictureVertices[2].X - pictureVertices[0].X);
            A[2] = (pictureVertices[3].Y - pictureVertices[0].Y) / (pictureVertices[3].X - pictureVertices[0].X);
            //Console.WriteLine("A1:{0}", A[0]);
            //Console.WriteLine("A2:{0}", A[1]);
            //Console.WriteLine("A3:{0}", A[2]);
            //Console.ReadLine();

            B[0] = (pictureVertices[0].X * realVertices[0].X - pictureVertices[1].X * realVertices[1].X) / (pictureVertices[1].X - pictureVertices[0].X);
            B[1] = (pictureVertices[0].X * realVertices[0].X - pictureVertices[2].X * realVertices[2].X) / (pictureVertices[2].X - pictureVertices[0].X);
            B[2] = (pictureVertices[0].X * realVertices[0].X - pictureVertices[3].X * realVertices[3].X) / (pictureVertices[3].X - pictureVertices[0].X);
            //Console.WriteLine("B1:{0}", B[0]);
            //Console.WriteLine("B2:{0}", B[1]);
            //Console.WriteLine("B3:{0}", B[2]);
            //Console.ReadLine();

            C[0] = (pictureVertices[0].Y * realVertices[0].X - pictureVertices[1].Y * realVertices[1].X) / (pictureVertices[1].X - pictureVertices[0].X);
            C[1] = (pictureVertices[0].Y * realVertices[0].X - pictureVertices[2].Y * realVertices[2].X) / (pictureVertices[2].X - pictureVertices[0].X);
            C[2] = (pictureVertices[0].Y * realVertices[0].X - pictureVertices[3].Y * realVertices[3].X) / (pictureVertices[3].X - pictureVertices[0].X);
            //Console.WriteLine("C1:{0}", C[0]);
            //Console.WriteLine("C2:{0}", C[1]);
            //Console.WriteLine("C3:{0}", C[2]);
            //Console.ReadLine();

            D[0] = (pictureVertices[0].X * realVertices[0].Y - pictureVertices[1].X * realVertices[1].Y) / (pictureVertices[1].X - pictureVertices[0].X);
            D[1] = (pictureVertices[0].X * realVertices[0].Y - pictureVertices[2].X * realVertices[2].Y) / (pictureVertices[2].X - pictureVertices[0].X);
            D[2] = (pictureVertices[0].X * realVertices[0].Y - pictureVertices[3].X * realVertices[3].Y) / (pictureVertices[3].X - pictureVertices[0].X);
            //Console.WriteLine("D1:{0}", D[0]);
            //Console.WriteLine("D2:{0}", D[1]);
            //Console.WriteLine("D3:{0}", D[2]);
            //Console.ReadLine();

            E[0] = (pictureVertices[0].Y * realVertices[0].Y - pictureVertices[1].Y * realVertices[1].Y) / (pictureVertices[1].X - pictureVertices[0].X);
            E[1] = (pictureVertices[0].Y * realVertices[0].Y - pictureVertices[2].Y * realVertices[2].Y) / (pictureVertices[2].X - pictureVertices[0].X);
            E[2] = (pictureVertices[0].Y * realVertices[0].Y - pictureVertices[3].Y * realVertices[3].Y) / (pictureVertices[3].X - pictureVertices[0].X);
            //Console.WriteLine("E1:{0}", E[0]);
            //Console.WriteLine("E2:{0}", E[1]);
            //Console.WriteLine("E3:{0}", E[2]);
            //Console.ReadLine();

            F[0] = (realVertices[1].X - realVertices[0].X) / (pictureVertices[1].X - pictureVertices[0].X);
            F[1] = (realVertices[2].X - realVertices[0].X) / (pictureVertices[2].X - pictureVertices[0].X);
            F[2] = (realVertices[3].X - realVertices[0].X) / (pictureVertices[3].X - pictureVertices[0].X);
            //Console.WriteLine("F1:{0}", F[0]);
            //Console.WriteLine("F2:{0}", F[1]);
            //Console.WriteLine("F3:{0}", F[2]);
            //Console.ReadLine();

            G[0] = (realVertices[1].Y - realVertices[0].Y) / (pictureVertices[1].X - pictureVertices[0].X);
            G[1] = (realVertices[2].Y - realVertices[0].Y) / (pictureVertices[2].X - pictureVertices[0].X);
            G[2] = (realVertices[3].Y - realVertices[0].Y) / (pictureVertices[3].X - pictureVertices[0].X);
            //Console.WriteLine("G1:{0}", G[0]);
            //Console.WriteLine("G2:{0}", G[1]);
            //Console.WriteLine("G3:{0}", G[2]);
            //Console.ReadLine();

            P[0] = (A[2] - A[0]) * (B[1] - B[0]) - (A[1] - A[0]) * (B[2] - B[0]);
            P[1] = (A[2] - A[0]) * (C[1] - C[0]) - (A[1] - A[0]) * (C[2] - C[0]);
            P[2] = (A[2] - A[0]) * (D[1] - D[0]) - (A[1] - A[0]) * (D[2] - D[0]);
            P[3] = (A[2] - A[0]) * (E[1] - E[0]) - (A[1] - A[0]) * (E[2] - E[0]);
            P[4] = (A[2] - A[0]) * (F[1] - F[0]) - (A[1] - A[0]) * (F[2] - F[0]);
            P[5] = (A[2] - A[0]) * (G[1] - G[0]) - (A[1] - A[0]) * (G[2] - G[0]);
            //Console.WriteLine("P1:{0}", P[0]);
            //Console.WriteLine("P2:{0}", P[1]);
            //Console.WriteLine("P3:{0}", P[2]);
            //Console.WriteLine("P4:{0}", P[3]);
            //Console.WriteLine("P5:{0}", P[4]);
            //Console.WriteLine("P6:{0}", P[5]);
            //Console.ReadLine();

            c[6, 0] = (P[3] * P[4] - P[1] * P[5]) / (P[0] * P[3] - P[1] * P[2]);
            c[7, 0] = (P[0] * P[5] - P[2] * P[4]) / (P[0] * P[3] - P[1] * P[2]);
            c[1, 0] = ((F[1] - F[0]) - (B[1] - B[0]) * c[6, 0] - (C[1] - C[0]) * c[7, 0]) / (A[1] - A[0]);
            c[4, 0] = ((G[1] - G[0]) - (D[1] - D[0]) * c[6, 0] - (E[1] - E[0]) * c[7, 0]) / (A[1] - A[0]);
            c[0, 0] = F[0] - A[0] * c[1, 0] - B[0] * c[6, 0] - C[0] * c[7, 0];
            c[3, 0] = G[0] - A[0] * c[4, 0] - D[0] * c[6, 0] - E[0] * c[7, 0];
            c[2, 0] = realVertices[0].X - pictureVertices[0].X * c[0, 0] - pictureVertices[0].Y * c[1, 0] + pictureVertices[0].X * realVertices[0].X * c[6, 0] + pictureVertices[0].Y * realVertices[0].X * c[7, 0];
            c[5, 0] = realVertices[2].Y - pictureVertices[2].X * c[3, 0] - pictureVertices[2].Y * c[4, 0] + pictureVertices[2].X * realVertices[2].Y * c[6, 0] + pictureVertices[2].Y * realVertices[2].Y * c[7, 0];
            //Console.WriteLine("c1:{0}", c[0, 0]);
            //Console.WriteLine("c2:{0}", c[1, 0]);
            //Console.WriteLine("c3:{0}", c[2, 0]);
            //Console.WriteLine("c4:{0}", c[3, 0]);
            //Console.WriteLine("c5:{0}", c[4, 0]);
            //Console.WriteLine("c6:{0}", c[5, 0]);
            //Console.WriteLine("c7:{0}", c[6, 0]);
            //Console.WriteLine("c8:{0}", c[7, 0]);
            //Console.ReadLine();
        }

        /// <summary>
        /// This method uses the parameters from ProjectiveUnControlled and if there are more then 4 veritces then it refines the parameters using least squares estimation.
        /// </summary>
        /// <param name="pictureVertices">These are the vertices on the picture that is being digitized.</param>
        /// <param name="realVertices">These vertices are the real coordinates of the vertices selected on the picture in the same order.</param>
        public Projective(point[] pictureVertices, point[] realVertices)
        {
            if (pictureVertices.GetLength(0) >= 4)
            {
                ProjectiveUnControlled(pictureVertices, realVertices);
                if (pictureVertices.GetLength(0) > 4)
                {
                    oldc = c;
                    Matrice Kc = new Matrice(pictureVertices.GetLength(0) * 2, 8);
                    Matrice realX = new Matrice(pictureVertices.GetLength(0) * 2, 1); // x+y matrice created using the given realVertices parameter
                    Matrice vertices = new Matrice(pictureVertices.GetLength(0), 2);
                    Matrice A = new Matrice(pictureVertices.GetLength(0) * 2, pictureVertices.GetLength(0) * 2);
                    Matrice dX;
                    Matrice R; // Co-efficient of dX
                    Matrice T; // Co-efficient of dC
                    Matrice dC;
                Iterate:
                    for (int i = 0; i < pictureVertices.GetLength(0); i++)
                    {
                        Kc[i, 0] = pictureVertices[i].X;
                        Kc[i + pictureVertices.GetLength(0), 3] = pictureVertices[i].X;
                        Kc[i, 1] = pictureVertices[i].Y;
                        Kc[i + pictureVertices.GetLength(0), 4] = pictureVertices[i].Y;
                        Kc[i, 2] = 1;
                        Kc[i + pictureVertices.GetLength(0), 5] = 1;
                        Kc[i, 6] = -(pictureVertices[i].X * realVertices[i].X);
                        Kc[i, 7] = -(pictureVertices[i].Y * realVertices[i].X);
                        Kc[i + pictureVertices.GetLength(0), 6] = -(pictureVertices[i].X * realVertices[i].Y);
                        Kc[i + pictureVertices.GetLength(0), 7] = -(pictureVertices[i].Y * realVertices[i].Y);
                        realX[i, 0] = realVertices[i].X;
                        realX[i + pictureVertices.GetLength(0), 0] = realVertices[i].Y;
                        A[i, i] = pictureVertices[i].X * c[6, 0] + pictureVertices[i].Y * c[7, 0] + 1;
                        A[i + pictureVertices.GetLength(0), i + pictureVertices.GetLength(0)] = pictureVertices[i].X * c[6, 0] + pictureVertices[i].Y * c[7, 0] + 1;
                    }
                    dX = Kc * c - realX;

                    //  Kc.Transpose *  (A*A.Transpose)^-1  * Kc * dC = Kc.Transpose *  (A*A.Transpose)^-1  * dXReal
                    //  >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>  T * dC = R
                    //  >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>  dC = T^-1 * R
                    R = A * A.TransposeNoChange();
                    R = R.Inverse();
                    R = Kc.TransposeNoChange() * R;
                    T = R * Kc;
                    T.Inverse();
                    R = R * dX;
                    dC = T * R;
                    c = c - dC;
                    Matrice dummy = Kc * c;
                    Console.WriteLine(dC.Sum());
                    Console.WriteLine(dC.ToString());
                    Console.ReadKey();
                    Console.WriteLine(dummy.ToString());
                    Console.ReadKey();

                    if (Math.Abs(dC.Sum()) > 0.00000005)
                        goto Iterate;
                }
            }
        }
    }

    class ProjectiveTestClass
    {
        /// <summary>
        /// Main Function to be run when compiled as a console exe
        /// </summary>
        static void Main(string[] args)
        {
            point[] pictureVertices = new point[7];
            point[] realVertices = new point[7];
            /*
            Console.WriteLine("Picture Coordinates:");
            for (int i = 0; i < 4; i++)
            {
                Console.WriteLine("NN.{0}.X", (i + 1));
                pictureVertices[i].X = Convert.Todouble(Console.ReadLine());
                Console.WriteLine("NN.{0}.Y", (i + 1));
                pictureVertices[i].Y = Convert.Todouble(Console.ReadLine());
            }

            Console.WriteLine("Real Coordinates:");
            for (int i = 0; i < 4; i++)
            {
                Console.WriteLine("NN.{0}.X", (i + 1));
                realVertices[i].X = Convert.Todouble(Console.ReadLine());
                Console.WriteLine("NN.{0}.Y", (i + 1));
                realVertices[i].Y = Convert.Todouble(Console.ReadLine());
            }
            */
            pictureVertices[0].X = 5208.79;
            pictureVertices[1].X = 4833.49;
            pictureVertices[2].X = 5000.00;
            pictureVertices[3].X = 5166.91;
            pictureVertices[4].X = 5039.38;
            pictureVertices[5].X = 5050.71;
            pictureVertices[6].X = 4878.09;

            pictureVertices[0].Y = 5043.74;
            pictureVertices[1].Y = 5218.42;
            pictureVertices[2].Y = 5000.00;
            pictureVertices[3].Y = 5220.02;
            pictureVertices[4].Y = 5242.70;
            pictureVertices[5].Y = 5106.17;
            pictureVertices[6].Y = 5066.86;

            realVertices[0].X = 494068.83;
            realVertices[1].X = 491115.60;
            realVertices[2].X = 492553.38;
            realVertices[3].X = 493597.03;
            realVertices[4].X = 492626.70;
            realVertices[5].X = 492834.82;
            realVertices[6].X = 491584.95;

            realVertices[0].Y = 244618.72;
            realVertices[1].Y = 245579.41;
            realVertices[2].Y = 244103.98;
            realVertices[3].Y = 245893.38;
            realVertices[4].Y = 245946.69;
            realVertices[5].Y = 244940.44;
            realVertices[6].Y = 244491.45;

            Projective x = new Projective(pictureVertices, realVertices);
        }
    }
}
